#include "Quaternion.h"

namespace PocketSurvival::Engine
{
    void Quaternion::CreateFromAxisAngle(const Vector3 &axis, float angle, Quaternion &result)
    {
		float x = angle * 0.5f;
		float num = MathUtils::Sin<float>(x);
		float w = MathUtils::Cos<float>(x);
		result.X = axis.posX * num;
		result.Y = axis.posY * num;
		result.Z = axis.posZ * num;
		result.W = w;
    }

    void Quaternion::CreateFromRotationMatrix(Matrix &m, Quaternion &result)
    {
		float num = m.M11 + m.M22 + m.M33;
		if (num > 0.0f)
		{
			float num2 = MathUtils::Sqrt<float>(num + 1.0f);
			result.W = num2 * 0.5f;
			num2 = 0.5f / num2;
			result.X = (m.M23 - m.M32) * num2;
			result.Y = (m.M31 - m.M13) * num2;
			result.Z = (m.M12 - m.M21) * num2;
			return;
		}
		if (m.M11 >= m.M22 && m.M11 >= m.M33)
		{
			float num3 = MathUtils::Sqrt<float>(1.0f + m.M11 - m.M22 - m.M33);
			float num4 = 0.5f / num3;
			result.X = 0.5f * num3;
			result.Y = (m.M12 + m.M21) * num4;
			result.Z = (m.M13 + m.M31) * num4;
			result.W = (m.M23 - m.M32) * num4;
			return;
		}
		if (m.M22 > m.M33)
		{
			float num5 = MathUtils::Sqrt<float>(1.0f + m.M22 - m.M11 - m.M33);
			float num6 = 0.5f / num5;
			result.X = (m.M21 + m.M12) * num6;
			result.Y = 0.5f * num5;
			result.Z = (m.M32 + m.M23) * num6;
			result.W = (m.M31 - m.M13) * num6;
			return;
		}
		float num7 = MathUtils::Sqrt<float>(1.0f + m.M33 - m.M11 - m.M22);
		float num8 = 0.5f / num7;
		result.X = (m.M31 + m.M13) * num8;
		result.Y = (m.M32 + m.M23) * num8;
		result.Z = 0.5f * num7;
		result.W = (m.M12 - m.M21) * num8;
    }

    void Quaternion::CreateFromYawPitchRoll(float yaw, float pitch, float roll, Quaternion &result)
    {
			float x = roll * 0.5f;
			float x2 = pitch * 0.5f;
			float x3 = yaw * 0.5f;
			float num = MathUtils::Sin<float>(x);
			float num2 = MathUtils::Cos<float>(x);
			float num3 = MathUtils::Sin<float>(x2);
			float num4 = MathUtils::Cos<float>(x2);
			float num5 = MathUtils::Sin<float>(x3);
			float num6 = MathUtils::Cos<float>(x3);
            
            result.X = num6 * num3 * num2 + num5 * num4 * num;
            result.Y = num5 * num4 * num2 - num6 * num3 * num;
            result.Z = num6 * num4 * num - num5 * num3 * num2;
            result.W = num6 * num4 * num2 + num5 * num3 * num;
    }

	void Quaternion::Lerp(const Quaternion &q1, const Quaternion &q2, float f, Quaternion &result)
	{
		float num = 1.0f - f;
		if (q1.X * q2.X + q1.Y * q2.Y + q1.Z * q2.Z + q1.W * q2.W >= 0.0f)
		{
			result.X = num * q1.X + f * q2.X;
			result.Y = num * q1.Y + f * q2.Y;
			result.Z = num * q1.Z + f * q2.Z;
			result.W = num * q1.W + f * q2.W;
		}
		else
		{
			result.X = num * q1.X - f * q2.X;
			result.Y = num * q1.Y - f * q2.Y;
			result.Z = num * q1.Z - f * q2.Z;
			result.W = num * q1.W - f * q2.W;
		}
		float num2 = 1.0f / result.length();
		result.X *= num2;
		result.Y *= num2;
		result.Z *= num2;
		result.W *= num2;
	}

	void Quaternion::Slerp(const Quaternion &q1, const Quaternion &q2, float f, Quaternion &result)
	{
		float num = q1.X * q2.X + q1.Y * q2.Y + q1.Z * q2.Z + q1.W * q2.W;
		bool flag = false;
		if (num < 0.0f)
		{
			flag = true;
			num = 0.0f - num;
		}
		float num2;
		float num3;
		if (num > 0.999999f)
		{
			num2 = 1.0f - f;
			num3 = (flag ? (0.0f - f) : f);
		}
		else
		{
			float num4 = MathUtils::Acos<float>(num);
			float num5 = 1.0f / MathUtils::Sin<float>(num4);
			num2 = MathUtils::Sin<float>((1.0f - f) * num4) * num5;
			num3 = (flag ? ((0.0f - MathUtils::Sin<float>(f * num4)) * num5) : (MathUtils::Sin<float>(f * num4) * num5));
		}
		result.X = num2 * q1.X + num3 * q2.X;
		result.Y = num2 * q1.Y + num3 * q2.Y;
		result.Z = num2 * q1.Z + num3 * q2.Z;
		result.W = num2 * q1.W + num3 * q2.W;
	}

	Matrix Quaternion::toMatrix() const
	{
		float num = X * X;
		float num2 = Y * Y;
		float num3 = Z * Z;
		float num4 = X * Y;
		float num5 = Z * W;
		float num6 = X * Z;
		float num7 = Y * W;
		float num8 = Y * Z;
		float num9 = X * W;

		return Matrix {
			1.0f - 2.0f * (num2 + num3),
			2.0f * (num4 + num5),
			2.0f * (num6 - num7),
			0.0f,
			2.0f * (num4 - num5),
			1.0f - 2.0f * (num3 + num),
			2.0f * (num8 + num9),
			0.0f,
			2.0f * (num6 + num7),
			2.0f * (num8 - num9),
			1.0f - 2.0f * (num2 + num),
			0.0f,
			0.0f,
			0.0f,
			0.0f,
			1.0f };
	}

	Vector3 Quaternion::toYawPitchRoll() const
	{
		float num = -2.0f * (Y * W + X * Z);
		float x = 2.0f * (X * W - Y * Z);
		float num2 = 2.0f * (X * X + Y * Y) - 1.0f;
		float y = 2.0f * (X * Y + Z * W);
		float x2 = 1.0f - 2.0f * (X * X + Z * Z);
		float x3 = MathUtils::Atan2<float>(0.0f - num, 0.0f - num2);
		float y2 = MathUtils::Asin<float>(x);
		float z = MathUtils::Atan2<float>(y, x2);
		return Vector3 { x3, y2, z };
	}

	Vector3 Quaternion::Vector3Transform(const Vector3 &v, const Quaternion &q)
	{
		float num = q.X + q.X;
		float num2 = q.Y + q.Y;
		float num3 = q.Z + q.Z;
		float num4 = q.W * num;
		float num5 = q.W * num2;
		float num6 = q.W * num3;
		float num7 = q.X * num;
		float num8 = q.X * num2;
		float num9 = q.X * num3;
		float num10 = q.Y * num2;
		float num11 = q.Y * num3;
		float num12 = q.Z * num3;
		return Vector3(
			(v.posX * (1.0f - num10 - num12)) + (v.posY * (num8 - num6)) + (v.posZ * (num9 + num5)),
			(v.posX * (num8 + num6)) + (v.posY * (1.0f - num7 - num12)) + (v.posZ * (num11 - num4)), 
			(v.posX * (num9 - num5)) + (v.posY * (num11 + num4)) + (v.posZ * (1.0f - num7 - num10)));
	}

} // namespace PocketSurvival
